#include "formation_avoidance/formation.h"

double resolution = 0.1;
double trunc_distance = 1.5;
const int POW = 7;	
const int N = (1 << POW);

ewok::EuclideanDistanceCheckingRingBuffer<POW> rrb(resolution,trunc_distance); 

double GridMapNode::getdistance()
{
    Eigen::Vector3f path_point;
    rrb.get_distance_point(index,path_point);
	return rrb.get_edf_distance(path_point.cast <float> ());
}

bool GridMapNode::checkOccupancy()
{
	return rrb.CheckOccupied(index);
}

double GridMapNode::NodeSafety()
{
    double safety=0;
    if(rrb.CheckKnown(index)) safety=10;
    return safety;
}

double Formation_Agent::getDiagHeu(GridMapNode* node1, GridMapNode* node2)
{
    double dx = abs(node1->index(0) - node2->index(0));
    double dy = abs(node1->index(1) - node2->index(1));
    double dz = abs(node1->index(2) - node2->index(2));

    double h = 0.0;
    int diag = std::min(std::min(dx, dy), dz);
    dx -= diag;
    dy -= diag;
    dz -= diag;

    if (dx == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dy, dz) + 1.0 * abs(dy - dz);
    }
    if (dy == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dz) + 1.0 * abs(dx - dz);
    }
    if (dz == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dy) + 1.0 * abs(dx - dy);
    }
    return h;
}

void Formation_Agent::visual_a_star_path()
{
    nav_msgs::Path a_star_path;
    a_star_path.poses.clear();

    a_star_path.header.stamp=ros::Time::now();
    a_star_path.header.frame_id="world";

    if(a_star_gridpath.size()==0) return;
    for(int i=0; i<a_star_gridpath.size(); i++)
    {
        geometry_msgs::PoseStamped point;

        Eigen::Vector3i path_idx=a_star_gridpath[i]->index;
        // std::cout<<"path_idx: "<<path_idx<<std::endl;
        Eigen::Vector3f path_point;
        rrb.get_distance_point(path_idx,path_point);
        point.pose.position.x = path_point(0);

        point.pose.position.y= path_point(1);
        point.pose.position.z = path_point(2);

        point.header.stamp=ros::Time::now();
        point.header.frame_id="world";
        a_star_path.poses.push_back(point);        
    }
    while(1)
    {
        const int LOOPRATE = 30;
        ros::Rate loop_rate(LOOPRATE);  
        a_star_pub.publish(a_star_path);
        loop_rate.sleep();
    }

}


void Formation_Agent::a_star_retrievePath(GridMapNode* final_node)
{
    a_star_gridpath.clear();
    a_star_gridpath.push_back(final_node);

    while (final_node->parent_node != NULL)
    {
        final_node = final_node->parent_node ;
        a_star_gridpath.push_back(final_node);
    }

}

double Formation_Agent::getpointdistance(GridMapNode* node1, GridMapNode* node2)
{
    return (node1->index-node2->index).cast <float> ().norm();
}

void Formation_Agent::a_star_RemoveCollinearPath()
{
    std::vector<GridMapNode*> tmp_path=a_star_gridpath;
    a_star_gridpath.clear();

    a_star_gridpath.push_back(*tmp_path.begin());

    for (std::vector<GridMapNode*>::const_iterator iter = tmp_path.begin(); iter != tmp_path.end()-2; iter++)
    {
        // ROS_INFO("SSSSS");
        // GridMapNode* node1=*(iter+2);
        // std::cout<<"*(iter+2)->index(): "<<node1->index<<std::endl;
        double distance13 = getpointdistance(*(iter+2), *iter);
        double distance12 = getpointdistance(*(iter+1), *iter);
        double distance23 = getpointdistance(*(iter+2),*(iter+1));
        if (abs(distance13 - distance12 - distance23)  ==0) 
        {
            // ROS_INFO("distance %f %f %f",distance13,distance12,distance23);
            continue;
        }
        a_star_gridpath.push_back(*(iter+1));
    }
    a_star_gridpath.push_back(*(tmp_path.end()-1));

    reverse(a_star_gridpath.begin(),a_star_gridpath.end());
    ROS_INFO("a_star_grid_path size after remove collinear point: %ld",a_star_gridpath.size());
}

std::vector<GridMapNode> GridMapNode::getbresenham3D(GridMapNode* bresenham3Dend_ptr)
{
    std::vector<GridMapNode> bresenham3Dpath;
    // bresenham3Dpath.push_back(this);
    // ListOfPoints.append((x1, y1, z1))

    int dx = abs(bresenham3Dend_ptr->index(0) - index(0) );
    int dy =abs(bresenham3Dend_ptr->index(1) - index(1) );
    int dz =abs(bresenham3Dend_ptr->index(2) - index(2) );
    int xs,ys,zs;
    xs=(bresenham3Dend_ptr->index(0)> index(0))?1:-1;
    ys=(bresenham3Dend_ptr->index(1)> index(1))?1:-1;
    zs=(bresenham3Dend_ptr->index(2)> index(2))?1:-1;

    int x1,y1,z1,x2,y2,z2;
    x1=index(0);    y1=index(1);    z1=index(2);
    x2=bresenham3Dend_ptr->index(0);    y2=bresenham3Dend_ptr->index(1);    z2=bresenham3Dend_ptr->index(2);

    int p1,p2;
    // # Driving axis is X-axis"
    if (dx >= dy && dx >= dz)
    {
        p1 = 2 * dy - dx;
        p2 = 2 * dz - dx;
        while (x1 != x2)
        {
            x1 += xs;
            if (p1 >= 0)
            {
                y1 += ys;
                p1 -= 2 * dx;
            }

            if (p2 >= 0)
            {
                z1 += zs;
                p2 -= 2 * dx;
            }

            p1 += 2 * dy;
            p2 += 2 * dz;
            Eigen::Vector3i tmp(x1,y1,z1);
            GridMapNode tmp_node(tmp);
            bresenham3Dpath.push_back(tmp_node);
        }

    }

    // # Driving axis is Y-axis"
    else if (dy >= dx and dy >= dz)
    {
        p1 = 2 * dx - dy;
        p2 = 2 * dz - dy;
        while (y1 != y2)
        {
            y1 += ys;
            if (p1 >= 0)
            {
                x1 += xs;
                p1 -= 2 * dy;
            }
            if (p2 >= 0)
            {
                z1 += zs;
                p2 -= 2 * dy;
            }
            p1 += 2 * dx;
            p2 += 2 * dz;
            Eigen::Vector3i tmp(x1,y1,z1);
            GridMapNode tmp_node(tmp);
            bresenham3Dpath.push_back(tmp_node);        
        }

    }

    // # Driving axis is Z-axis"
    else{
        p1 = 2 * dy - dz;
        p2 = 2 * dx - dz;
        while (z1 != z2)
        {
            z1 += zs;
            if (p1 >= 0)
            {
                y1 += ys;
                p1 -= 2 * dz;
            }
            if (p2 >= 0)
            {
                x1 += xs;
                p2 -= 2 * dz;
            }
            p1 += 2 * dy;
            p2 += 2 * dx;
            Eigen::Vector3i tmp(x1,y1,z1);
            GridMapNode tmp_node(tmp);
            bresenham3Dpath.push_back(tmp_node);        
        }
    }

    return bresenham3Dpath;

}


void Formation_Agent::a_star_bresenham3D()
{
    std::vector<GridMapNode*> tmp_path=a_star_gridpath;
    a_star_gridpath.clear();
    a_star_gridpath.push_back(*tmp_path.begin());

    GridMapNode* bresenham3Dstart_ptr=*tmp_path.begin();

    int j=0;
    for(j=1;j<tmp_path.size();j++)
    {
        std::vector<GridMapNode> bresenham3Dpath=bresenham3Dstart_ptr->getbresenham3D(tmp_path[j]);
        for(int i=0;i<bresenham3Dpath.size();i++)
        {
            int tmp=i*1;
            if(tmp>=bresenham3Dpath.size()) break;
            if(bresenham3Dpath[i].getdistance()<0.5)
            {
                a_star_gridpath.push_back(tmp_path[j-1]);
                bresenham3Dstart_ptr=tmp_path[j-1];
                break;
            }
        }
    }
    a_star_gridpath.push_back(*(tmp_path.end()-1));
    ROS_INFO("a_star_grid_path size after bresenham3D: %ld",a_star_gridpath.size());



}

bool GridCube::VertexIsContained(Eigen::MatrixXi last_InflateCube)
{
    if( vertex_idx(0, 0) <= last_InflateCube(0, 0) && vertex_idx(0, 1) >= last_InflateCube(0, 1) && vertex_idx(0, 2) <= last_InflateCube(0, 2) &&
        vertex_idx(6, 0) >= last_InflateCube(6, 0) && vertex_idx(6, 1) <= last_InflateCube(6, 1) && vertex_idx(6, 2) >= last_InflateCube(6, 2)  )
        return true;
    else
        return false; 
}


bool GridCube::InflateCube(Eigen::MatrixXi last_InflateCube)
{

    // Y- now is the left side : (p1 -- p4 -- p8 -- p5) face sweep
    // ############################################################################################################
    bool collide;

    int iter = 0;
    int _max_inflate_iter=10;
    int _step_length=2;
    int id_y,id_x,id_z;
    int _max_y_id=30;
    int _max_x_id=30;
    int _max_z_id=10;
    Eigen::MatrixXi vertex_idx_lst = vertex_idx;
    if(VertexIsContained(last_InflateCube))return false;
    Eigen::MatrixXi initial_vertex=center_idx;
    double safedistance_corridor=0.3;

    while(iter < _max_inflate_iter)
    {   
        iter++;
        collide  = false; 
        int y_lo = std::max(initial_vertex(1)-_max_y_id, vertex_idx(0, 1) - _step_length);
        int y_up = std::min(initial_vertex(1)+_max_y_id, vertex_idx(1, 1) + _step_length);

        for(id_y = vertex_idx(0, 1); id_y >= y_lo; id_y-- )
        {   
            if( collide == true) 
                break;
            
            for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
            {    
                if( collide == true) 
                    break;

                for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(0, 1) = std::min(id_y+2, vertex_idx(0, 1));
            vertex_idx(3, 1) = std::min(id_y+2, vertex_idx(3, 1));
            vertex_idx(7, 1) = std::min(id_y+2, vertex_idx(7, 1));
            vertex_idx(4, 1) = std::min(id_y+2, vertex_idx(4, 1));
        }
        else
            vertex_idx(0, 1) = vertex_idx(3, 1) = vertex_idx(7, 1) = vertex_idx(4, 1) = id_y + 1;

        // Y+ now is the right side : (p2 -- p3 -- p7 -- p6) face
        // ############################################################################################################
        collide = false;
        for(id_y = vertex_idx(1, 1); id_y <= y_up; id_y++ )
        {   
            if( collide == true) 
                break;
            
            for(id_x = vertex_idx(1, 0); id_x >= vertex_idx(2, 0); id_x-- )
            {
                if( collide == true) 
                    break;

                for(id_z = vertex_idx(1, 2); id_z >= vertex_idx(5, 2); id_z-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(1, 1) = max(id_y-2, vertex_idx(1, 1));
            vertex_idx(2, 1) = max(id_y-2, vertex_idx(2, 1));
            vertex_idx(6, 1) = max(id_y-2, vertex_idx(6, 1));
            vertex_idx(5, 1) = max(id_y-2, vertex_idx(5, 1));
        }
        else
            vertex_idx(1, 1) = vertex_idx(2, 1) = vertex_idx(6, 1) = vertex_idx(5, 1) = id_y - 1;

        // X + now is the front side : (p1 -- p2 -- p6 -- p5) face
        // ############################################################################################################
        int x_lo = max(initial_vertex(0)-_max_x_id, vertex_idx(3, 0) - _step_length);
        int x_up = min(initial_vertex(0)+_max_x_id, vertex_idx(0, 0) + _step_length);

        collide = false;
        for(id_x = vertex_idx(0, 0); id_x <= x_up; id_x++ )
        {   
            if( collide == true) 
                break;
            
            for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
            {
                if( collide == true) 
                    break;

                for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            
            }
        }

        if(collide)
        {
            vertex_idx(0, 0) = max(id_x-2, vertex_idx(0, 0)); 
            vertex_idx(1, 0) = max(id_x-2, vertex_idx(1, 0)); 
            vertex_idx(5, 0) = max(id_x-2, vertex_idx(5, 0)); 
            vertex_idx(4, 0) = max(id_x-2, vertex_idx(4, 0)); 
        }
        else
            vertex_idx(0, 0) = vertex_idx(1, 0) = vertex_idx(5, 0) = vertex_idx(4, 0) = id_x - 1;    

        // X- now is the back side : (p4 -- p3 -- p7 -- p8) face
        // ############################################################################################################
        collide = false;
        for(id_x = vertex_idx(3, 0); id_x >= x_lo; id_x-- )
        {   
            if( collide == true) 
                break;
            
            for(id_y = vertex_idx(3, 1); id_y <= vertex_idx(2, 1); id_y++ )
            {
                if( collide == true) 
                    break;

                for(id_z = vertex_idx(3, 2); id_z >= vertex_idx(7, 2); id_z-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(3, 0) = min(id_x+2, vertex_idx(3, 0)); 
            vertex_idx(2, 0) = min(id_x+2, vertex_idx(2, 0)); 
            vertex_idx(6, 0) = min(id_x+2, vertex_idx(6, 0)); 
            vertex_idx(7, 0) = min(id_x+2, vertex_idx(7, 0)); 
        }
        else
            vertex_idx(3, 0) = vertex_idx(2, 0) = vertex_idx(6, 0) = vertex_idx(7, 0) = id_x + 1;
    
        // Z+ now is the above side : (p1 -- p2 -- p3 -- p4) face
        // ############################################################################################################
        collide = false;
        int z_lo = max(initial_vertex(2)-_max_z_id, vertex_idx(4, 2) - _step_length);
        int z_up = min(initial_vertex(2)+_max_z_id, vertex_idx(0, 2) + _step_length);
        for(id_z = vertex_idx(0, 2); id_z <= z_up; id_z++ )
        {   
            if( collide == true) 
                break;
            
            for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
            {
                if( collide == true) 
                    break;

                for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(0, 2) = max(id_z-2, vertex_idx(0, 2));
            vertex_idx(1, 2) = max(id_z-2, vertex_idx(1, 2));
            vertex_idx(2, 2) = max(id_z-2, vertex_idx(2, 2));
            vertex_idx(3, 2) = max(id_z-2, vertex_idx(3, 2));
        }
        vertex_idx(0, 2) = vertex_idx(1, 2) = vertex_idx(2, 2) = vertex_idx(3, 2) = id_z - 1;

        // now is the below side : (p5 -- p6 -- p7 -- p8) face
        // ############################################################################################################
        collide = false;
        for(id_z = vertex_idx(4, 2); id_z >= z_lo; id_z-- )
        {   
            if( collide == true) 
                break;
            
            for(id_y = vertex_idx(4, 1); id_y <= vertex_idx(5, 1); id_y++ )
            {
                if( collide == true) 
                    break;

                for(id_x = vertex_idx(4, 0); id_x >= vertex_idx(7, 0); id_x-- )
                {
                    Eigen::Vector3i idx(id_x,id_y,id_z);
                    GridMapNode check_node(idx);
                    if(check_node.getdistance()<safedistance_corridor)
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(4, 2) = min(id_z+2, vertex_idx(4, 2));
            vertex_idx(5, 2) = min(id_z+2, vertex_idx(5, 2));
            vertex_idx(6, 2) = min(id_z+2, vertex_idx(6, 2));
            vertex_idx(7, 2) = min(id_z+2, vertex_idx(7, 2));
        }
        else
            vertex_idx(4, 2) = vertex_idx(5, 2) = vertex_idx(6, 2) = vertex_idx(7, 2) = id_z + 1;

        if(vertex_idx_lst == vertex_idx)break;
        vertex_idx_lst = vertex_idx;

        if(VertexIsContained(last_InflateCube))return false;
    }
    return true;

}

void Formation_Agent::visual_corridor()
{
    visualization_msgs::MarkerArray cube_vis;

    for(auto & mk: cube_vis.markers) 
    mk.action = visualization_msgs::Marker::DELETE;
    
    _corridor_vis_pub.publish(cube_vis);

    cube_vis.markers.clear();

    visualization_msgs::Marker mk;
    mk.header.frame_id = "world";
    mk.header.stamp = ros::Time::now();
    mk.ns = "corridor";
    mk.type = visualization_msgs::Marker::CUBE;
    mk.action = visualization_msgs::Marker::ADD;

    mk.pose.orientation.x = 0.0;
    mk.pose.orientation.y = 0.0;
    mk.pose.orientation.z = 0.0;
    mk.pose.orientation.w = 1.0;

    mk.color.a = 0.4;
    mk.color.r = 1.0;
    mk.color.g = 1.0;
    mk.color.b = 1.0;

    int idx = 10;
    double _is_proj_cube=false;
    for(int i = 0; i < int(CorridorCube.size()); i++)
    {   
        mk.color.r = i/CorridorCube.size();
        mk.id = idx;

        mk.pose.position.x = (CorridorCube[i].vertex_pos(0, 0) + CorridorCube[i].vertex_pos(3, 0) ) / 2.0; 
        mk.pose.position.y = (CorridorCube[i].vertex_pos(0, 1) + CorridorCube[i].vertex_pos(1, 1) ) / 2.0; 

        if(_is_proj_cube)
            mk.pose.position.z = 0.0; 
        else
            mk.pose.position.z = (CorridorCube[i].vertex_pos(0, 2) + CorridorCube[i].vertex_pos(4, 2) ) / 2.0; 

        mk.scale.x = abs(CorridorCube[i].vertex_pos(0, 0) - CorridorCube[i].vertex_pos(3, 0) );
        mk.scale.y = abs(CorridorCube[i].vertex_pos(1, 1) - CorridorCube[i].vertex_pos(0, 1) );

        if(_is_proj_cube)
            mk.scale.z = 0.05; 
        else
            mk.scale.z = abs(CorridorCube[i].vertex_pos(0, 2) - CorridorCube[i].vertex_pos(4, 2) );

        idx ++;
        cube_vis.markers.push_back(mk);
    }

    _corridor_vis_pub.publish(cube_vis);
}

void GridCube::UpdateVertexPos()
{
    for(int i=0;i<8;i++)
    {
        Eigen::Vector3f tmp(0,0,0);
        rrb.get_distance_point(vertex_idx.row(i),tmp);
        vertex_pos.row(i)=tmp;
    }
}


bool Formation_Agent::corridor_generation()
{
    Eigen::MatrixXi last_InflateCube=Eigen::MatrixXi::Zero(8,3);
    for(int i=0;i<a_star_gridpath.size();i++)
    {
        GridMapNode* node=a_star_gridpath[i];
        GridCube initial_cube(node->index);
        if(initial_cube.InflateCube(last_InflateCube))
        {
            initial_cube.UpdateVertexPos();
            CorridorCube.push_back(initial_cube);
            last_InflateCube=initial_cube.vertex_idx;
        }
    }
    visual_corridor();
    ROS_INFO("CorridorCube.size(): %ld",CorridorCube.size());
    return true;
}



bool Formation_Agent::a_star_search(Eigen::Vector3f start_p,Eigen::Vector3f end_p)
{
    std::vector<std::vector<std::vector<GridMapNodePtr> > > GridMap_(pool_size,std::vector<std::vector<GridMapNodePtr> >(pool_size,std::vector<GridMapNodePtr>(pool_size,0)));
	for(int z=0;z<pool_size;z++)
	{
		for (int y=0;y<pool_size;y++)
		{
			for (int x=0;x<pool_size;x++)
			{
				GridMap_[x][y][z]=new GridMapNode;
			}
		}
	}

	// GridMapNodePtr ***GridMap_;

    // Eigen::Vector3i POOL_SIZE_(pool_size,pool_size,pool_size);

    // GridMap_ = new GridMapNodePtr **[POOL_SIZE_(0)];
    // for (int i = 0; i < POOL_SIZE_(0); i++)
    // {
    //     GridMap_[i] = new GridMapNodePtr *[POOL_SIZE_(1)];
    //     for (int j = 0; j < POOL_SIZE_(1); j++)
    //     {
    //         GridMap_[i][j] = new GridMapNodePtr[POOL_SIZE_(2)];
    //         for (int k = 0; k < POOL_SIZE_(2); k++)
    //         {
    //             GridMap_[i][j][k] = new GridMapNode;
    //         }
    //     }
    // }

    if (!map_initialized)return false;
    rounds_++;
	std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> empty;
    std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> openSet_;

    openSet_.swap(empty);
    ros::Time time_1 = ros::Time::now();

	Eigen::Vector3i start_index;
	Eigen::Vector3i end_index;

    rrb.get_distance_idx(start_p,start_index);
	std::cout<<"start_index: \n"<< start_index<<std::endl;


    rrb.get_distance_idx(end_p,end_index);
	std::cout<<"end_index: \n"<< end_index<<std::endl;

    // return true;
    // ROS_INFO("ssssssssss");
    Eigen::Vector3i centre(pool_size/2,pool_size/2,pool_size/2);
    Eigen::Vector3i offset=start_index-centre;

	GridMapNode* start_node=GridMap_[pool_size/2][pool_size/2][pool_size/2];
    // GridMapNode* end_node=GridMap_[end_index(0)-offset(0)][end_index(1)-offset(1)][end_index(2)-offset(2)];
    GridMapNode* end_node=new GridMapNode;
    for(int i=0;i<3;i++)
    {
        start_node->index(i) = start_index(i);
        end_node->index(i) = end_index(i);
    }
    start_node->rounds = rounds_;
    start_node->g_score = 0;
    // ROS_INFO("bbbbbbbbbbbbbbb");

    start_node->f_score = getHeu(start_node, end_node);
    // ROS_INFO("AAAAAAAAAAAAA");
    start_node->state = GridMapNode::OPENSET; //put start node in open set
    start_node->parent_node = NULL;

    openSet_.push(start_node); //put start in open set
    int num_iter=0;

    std::vector<Eigen::Vector3i> closeSet_idx;
    GridMapNode* current_node;
    GridMapNode* neighborPtr;

    ROS_INFO("GO INTO A STAR LOOP");
	while(!openSet_.empty())
	{
        num_iter++;
		current_node=openSet_.top();
        openSet_.pop();
        // ROS_INFO("///////////current_node->index: %d %d %d ",current_node->index(0),current_node->index(1),current_node->index(2));

        if (current_node->index(0) == end_node->index(0) && current_node->index(1) == end_node->index(1) && current_node->index(2) == end_node->index(2))
        {
            // ros::Time time_2 = ros::Time::now();
            // printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
            // if((time_2 - time_1).toSec() > 0.1)
            //     ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
            ROS_INFO("A STAR FIND PATH");
            a_star_retrievePath(current_node);
            a_star_RemoveCollinearPath();
            a_star_bresenham3D();
            corridor_generation();
            break;
        }

        // for (std::vector<Eigen::Vector3i>::const_iterator iter = closeSet_idx.end(); iter != closeSet_idx.begin(); iter--)
        // {
        //     if((*iter)==current_node->index)
        //     {
        //         ROS_WARN("???????????????????????????????????????????////");

        //         std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> queue_copy {openSet_}; // A copy for output
        //         while (!queue_copy.empty())
        //         {
        //             GridMapNode* tmp=queue_copy.top();
        //             ROS_INFO("OPENSET index: %d %d %d",tmp->index(0),tmp->index(1),tmp->index(2));
        //             queue_copy.pop();
        //         }

        //         break;
        //     }
        // }

        current_node->state = GridMapNode::CLOSEDSET; //move current node from open set to closed set.
        closeSet_idx.push_back(current_node->index);
		for (int dx = -1; dx <= 1; dx++)
        {
            for (int dy = -1; dy <= 1; dy++)
            {
                for (int dz = -1; dz <= 1; dz++)
                {
                    if (dx == 0 && dy == 0 && dz == 0) continue;
                    Eigen::Vector3i neighborIdx;
                    neighborIdx(0) = (current_node->index)(0) + dx;
                    neighborIdx(1) = (current_node->index)(1) + dy;
                    neighborIdx(2) = (current_node->index)(2) + dz;

                    // if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1)
                    // {
                    //     continue;
                    // }

                    neighborPtr = GridMap_[neighborIdx(0)-offset(0)][neighborIdx(1)-offset(1)][neighborIdx(2)-offset(2)];

                    neighborPtr->index = neighborIdx;

                    bool flag_explored = neighborPtr->rounds == rounds_;

                    if (flag_explored && neighborPtr->state == GridMapNode::CLOSEDSET)
                    {
                        continue; //in closed set.
                    }
                    neighborPtr->rounds = rounds_;

                    // if (neighborPtr->checkOccupancy())
                    if(neighborPtr->getdistance()<SAFETY_DISTANCE)
                    {
                        continue;
                    }
                    // flag_explored=false;
                    // for (std::vector<Eigen::Vector3i>::const_iterator iter = closeSet_idx.end(); iter != closeSet_idx.begin(); iter--)
                    // {
                    //     if((*iter)==neighborPtr->index)
                    //     {
                    //         flag_explored=true;
                    //         ROS_INFO("flag_explored:true--------------------");
                    //         break;
                    //     }
                    // }
                    // if(flag_explored)continue;

                    double static_cost = 0;
                    double data_tmp=0;
                    data_tmp= dx * dx + dy * dy + dz * dz;
                    static_cost=sqrt(data_tmp);
                    double tentative_gScore = current_node->g_score + static_cost;

                    if (!flag_explored)
                    {
                        //discover a new node
                        neighborPtr->state = GridMapNode::OPENSET;
                        neighborPtr->parent_node = current_node;
                        neighborPtr->g_score = tentative_gScore;
                        neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                        openSet_.push(neighborPtr); //put neighbor in open set and record it.
                        // ROS_INFO("neighborPtr->index: %d %d %d",neighborPtr->index(0),neighborPtr->index(1),neighborPtr->index(2));
                        if(neighborPtr->index==current_node->index)return true;

                    }
                    else if (tentative_gScore < neighborPtr->g_score)
                    { 
                        neighborPtr->parent_node = current_node;
                        neighborPtr->g_score = tentative_gScore;
                        neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                    }
                }
            }
        }
        ros::Time time_2 = ros::Time::now();
        if ((time_2 - time_1).toSec() > 0.2)
        {
            ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
            return false;
        }

	}
    ros::Time time_2 = ros::Time::now();
    ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter);
    visual_a_star_path();

    delete end_node;
    for(int z=0;z<pool_size;z++)
	{
		for (int y=0;y<pool_size;y++)
		{
			for (int x=0;x<pool_size;x++)
			{
				delete GridMap_[z][y][x];
			}
		}
	}

    return true;

}

void Formation_Agent::init(ros::NodeHandle &nh)
{

    cloud_sub=nh.subscribe("/iris/camera/depth/points", 1, &Formation_Agent::cloudCallback,this);
	cloud_visual_pub2=nh.advertise<sensor_msgs::PointCloud2>("/ring_buffer/cloud_ob2", 1, true);           //map cloud

	pose_sub=nh.subscribe("/mavros/local_position/pose", 1, &Formation_Agent::poseCallback,this);
    
    current_goal_position_visual_pub = nh.advertise<visualization_msgs::Marker>("/current_goal_position_visual", 1);
    a_star_pub = nh.advertise<nav_msgs::Path>("a_start_path", 1);
    _corridor_vis_pub = nh.advertise<visualization_msgs::MarkerArray>("/corridor", 1);

    ID=0;

    motor_yaw=0;
	init_head_yaw=0;
	CLOUD_DELAY=0;

    D435_T265_OFFSET_X=0.1;     //D435在T265前方为正
    D435_T265_OFFSET_Z=0.0;     //D435在T265上方为正

    D435_BASE_OFFSET_X=0.0;     //D435在云台基座前方为正
    D435_BASE_OFFSET_Z=0.0;    //D435在云台基座上方为正


    T265_OFFSET_X=0.0;        //T265在无人机中心前方为正
    T265_OFFSET_Y=0.0;        //T265在无人机中心左边为正
    T265_OFFSET_Z=0.0;        //#T265在无人机中心上方为正

    CLIP_DISTANCE=0.01;         //点云阶段距离
    map_initialized=false;

    cloud_filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
	cloud_clip.reset(new pcl::PointCloud<pcl::PointXYZ>());

}



void Formation_Agent::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
	ROS_INFO("get point cloud---");
    if(motor_yaw_queue.size()!=0)
    {
        for(int i=0;i<motor_yaw_queue.size();i++)
        {   //Sychronize pose by queue
            double time_stamp_motor_yaw = motor_yaw_queue[i].header.stamp.toSec();
            if(time_stamp_motor_yaw >= cloud->header.stamp.toSec()-CLOUD_DELAY)
            {
                motor_yaw = motor_yaw_queue[i].process_value-init_head_yaw;
                // ROS_INFO("motor_yaw:  %f",motor_yaw);
                // ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - time_stamp_pose);

                // ROS_INFO("cloud->header.stamp.toSec(): %f time_stamp_pose: %f pose_timestamp_queue.back(): %f motor_yaw_time: %f motor_yaw: %f",
                //         cloud->header.stamp.toSec(),time_stamp_pose,pose_timestamp_queue.back(),motor_yaw_time,motor_yaw);
                break;
            }
        }
    }

	while(motor_yaw > PI){
		motor_yaw -= PIx2;
	}
	while(motor_yaw < -PI){
		motor_yaw += PIx2;
	}

	geometry_msgs::Point32 real_motor_pv;
	real_motor_pv.x = -motor_yaw+init_head_yaw;
	real_motor_pv.y = 0;
	real_motor_pv.z = 0;

	// motor_pub.publish(real_motor_pv);
	

	geometry_msgs::PoseStamped pose_sychronized;
	Eigen::Quaterniond quaternion_sychronized=current_q[ID];  //w,x,y,z
	Eigen::Vector3d position_sychronized=current_p[ID];  

	while(!pose_queue.empty())
	{   
		pose_sychronized= pose_queue.front();
		if(pose_sychronized.header.stamp.toSec() >= cloud->header.stamp.toSec()-CLOUD_DELAY)
		{
			// ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - pose_temp.header.stamp.toSec());
			// ROS_INFO("cloud->header.stamp.toSec(): %fpose_temp.header.stamp.toSec(): %f pose_timestamp_queue.back(): %f motor_yaw_time: %f motor_yaw: %f",
			//         cloud->header.stamp.toSec(),time_stamp_pose,pose_timestamp_queue.back(),motor_yaw_time,motor_yaw);
			quaternion_sychronized.x()=pose_sychronized.pose.orientation.x;
			quaternion_sychronized.y()=pose_sychronized.pose.orientation.y;
			quaternion_sychronized.z()=pose_sychronized.pose.orientation.z;
			quaternion_sychronized.w()=pose_sychronized.pose.orientation.w;

			position_sychronized(0)=pose_sychronized.pose.position.x;
			position_sychronized(1)=pose_sychronized.pose.position.y;
			position_sychronized(2)=pose_sychronized.pose.position.z;
			break;
		}
		pose_queue.pop();
	}

	Eigen::Matrix4d vehicle2ground = Eigen::Matrix4d::Identity();
	vehicle2ground.block(0, 0, 3, 3) = Eigen::Matrix3d(quaternion_sychronized);
	vehicle2ground(0, 3) = position_sychronized(0);  //front direction
	vehicle2ground(1, 3) = position_sychronized(1);		//left direction
	vehicle2ground(2, 3) = position_sychronized(2);  //up direction

	Eigen::Matrix4d d435base2vehicle = Eigen::Matrix4d::Identity();
	Eigen::Quaterniond d435base2vehicle_rotate(cos(motor_yaw/2.0),0,0,sin(motor_yaw/2.0));
	d435base2vehicle.block(0, 0, 3, 3) = Eigen::Matrix3d(d435base2vehicle_rotate);
	d435base2vehicle(0, 3) = D435_T265_OFFSET_X;  //front direction
	d435base2vehicle(1, 3) = 0.0;					//left direction
	d435base2vehicle(2, 3) = D435_T265_OFFSET_Z ; //up direction


	Eigen::Matrix4d d4352d435base = Eigen::Matrix4d::Identity();
	Eigen::Quaterniond d4352d435base_rotate( 0.5,-0.5, 0.5,-0.5);  
	d4352d435base.block(0, 0, 3, 3) = Eigen::Matrix3d(d4352d435base_rotate);
	d4352d435base(0, 3) = D435_BASE_OFFSET_X;
	d4352d435base(1, 3) = 0;
	d4352d435base(2, 3) = D435_BASE_OFFSET_Z; 

	Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
	transform=vehicle2ground*d435base2vehicle*d4352d435base;  //d435 to ground

	// convert cloud to pcl form
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());

    // pcl::PCLPointCloud2::Ptr cloud_in (new pcl::PCLPointCloud2 ());

	pcl::fromROSMsg(*cloud, *cloud_in);

	// down-sample for all
	// ROS_INFO("111111111");
	pcl::VoxelGrid<pcl::PointXYZ> sor;
		// ROS_INFO("2222222");

    pcl::PCLPointCloud2::Ptr cloud_tmp (new pcl::PCLPointCloud2 ());

    // pcl::copyPointCloud (*cloud_in, *cloud_tmp);



	sor.setInputCloud(cloud_in);
		// ROS_INFO("3333333333");

	static float res = resolution;
		//ROS_INFO("444444");

	sor.setLeafSize(res, res, res);
		//ROS_INFO("55555555");

	sor.filter(*cloud_filtered);
		//ROS_INFO("66666666");

	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud (cloud_filtered);
	pass.setFilterFieldName ("z");// 滤波字段设置为z轴方向
	pass.setFilterLimits (0.0, CLIP_DISTANCE);
	// ROS_INFO("CLIP_DISTANCE: %f",CLIP_DISTANCE);
	pass.setFilterLimitsNegative (true); //设置保留范围内还是过滤掉范围内, 默认为flase，可以注释掉。true为过滤掉范围内的，flase为过滤掉范围外的
	pass.filter (*cloud_clip);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transform(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::transformPointCloud(*cloud_clip, *cloud_transform, transform);

	ewok::EuclideanDistanceCheckingRingBuffer<POW>::PointCloud cloud_ew;
	std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ> > points = cloud_transform->points; 

    // convert to ROS message and publish
    sensor_msgs::PointCloud2 cloud_visual;
    pcl::toROSMsg(*cloud_transform, cloud_visual);

    // message publish should have the same time stamp
    cloud_visual.header.stamp = ros::Time::now();
    cloud_visual.header.frame_id = "world";

	// ROS_INFO("points_size: %ld",points.size());

	Eigen::Vector3f origin(position_sychronized(0),position_sychronized(1),position_sychronized(2));

    for(int i = 0; i < points.size(); ++i)
    {
        double x_diff = fabs(origin(0) - points.at(i).x);
        double y_diff = fabs(origin(1) -points.at(i).y);
        double z_diff = fabs(origin(2)  - points.at(i).z);
        double noise_threshold = x_diff*x_diff + y_diff*y_diff + z_diff*z_diff;
        if (noise_threshold > 0.1)
        {
            Eigen::Vector3d point(points.at(i).x,points.at(i).y,points.at(i).z);
            cloud_ew.push_back(Eigen::Vector4f(points.at(i).x, points.at(i).y, points.at(i).z, 0));
			// std::cout<<"points: "<<points.at(i)<<std::endl;
        }
        else
        {
            // ROS_WARN("noise_threshold-----------------------");
        }
    }
	//ROS_INFO("cloud push back over");

	// initialize the ringbuffer map
	if(!map_initialized)
	{
		Eigen::Vector3i idx;
		rrb.getIdx(origin, idx);
		rrb.setOffset(idx);
		map_initialized = true;
		ROS_WARN("MAP Initialized!!");
	}
	else
	{
		// ROS_INFO("MAP HAS BEEN INITIALIZED--------");
		// move buffer when its center is not the same as UAV
		while(true)
		{
			Eigen::Vector3i origin_idx, offset, diff;
			rrb.getIdx(origin, origin_idx);
			offset = rrb.getVolumeCenter();
			//std::cout << "origin :" << origin_idx << " center:" << offset << std::endl;
			diff = origin_idx - offset;
			if(diff.array().any())
				rrb.moveVolume(diff.head<3>());
			else
				break;
		}
	}

	// insert point cloud to ringbuffer
	// std::cout<<"position_sychronized: "<<position_sychronized<<std::endl;
	// std::cout<<"origin: "<<origin<<std::endl;
	rrb.insertPointCloud(cloud_ew, origin);
	// ROS_INFO("INSERT HAS BEEN success--------");

	// Calculate distance field considering newly imported points (dynamic points)
	//rrb.updateDistanceDynamic(cloud_ew, origin);
	rrb.updateDistance();
	/* Update buffer for rolling head */
	// double head_update_start_time = ros::Time::now().toSec();
	// int heading_direction_seq = getHeadingSeq(motor_yaw);  //current heading direction
	// updateHeadBuffer(heading_direction_seq);
	// double head_update_time = ros::Time::now().toSec() - head_update_start_time;

	// ROS_INFO("/////////////////////");
	// ROS_INFO("/////////////////////");
	map_visualization();

	float distance=rrb.get_edf_distance(current_p[ID].cast <float> ());
	// ROS_ERROR("DISTANCE AT EDF MAP: %f ///////////////////",distance);

}


void Formation_Agent::map_visualization()
{
	//map_visulization
	if(!map_initialized) return;

    /*Obstacle cloud*/
    pcl::PointCloud<pcl::PointXYZ> cloud;
    Eigen::Vector3d center;

    rrb.getBufferAsCloud(cloud, center);

    // convert to ROS message and publish
    sensor_msgs::PointCloud2 cloud_visual;
    pcl::toROSMsg(cloud, cloud_visual);

    // message publish should have the same time stamp
    cloud_visual.header.stamp = ros::Time::now();
    cloud_visual.header.frame_id = "world";
    cloud_visual_pub2.publish(cloud_visual);

}


void Formation_Agent::pose_store(int index_temp,const geometry_msgs::PoseStampedPtr &msg )
{

    current_p[index_temp](0) = msg->pose.position.x;
    current_p[index_temp](1) = msg->pose.position.y;
    current_p[index_temp](2) = msg->pose.position.z;

    current_q[index_temp].x() = msg->pose.orientation.x;
    current_q[index_temp].y() = msg->pose.orientation.y;   
    current_q[index_temp].z() = msg->pose.orientation.z;
    current_q[index_temp].w() = msg->pose.orientation.w;

    pose_queue.push(*msg);
    if(pose_queue.size()>50)
    {
        pose_queue.pop();
    }
	
}

void Formation_Agent::poseCallback(const geometry_msgs::PoseStampedPtr &msg)
{
    int index_temp=0;
    pose_store(index_temp,msg);


    visualization_msgs::Marker points;

    points.id = 0;
    points.header.frame_id = "world";
    points.header.stamp = ros::Time::now();
    points.type = visualization_msgs::Marker::SPHERE_LIST;
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";

    points.scale.x = 0.2;
    points.scale.y = 0.2;
    points.scale.z = 0.2;

    points.lifetime = ros::Duration(0);

    geometry_msgs::Point point;
    point.x = current_p[ID](0);
    point.y = current_p[ID](1);
    point.z = current_p[ID](2);
    points.points.push_back(point);



    std_msgs::ColorRGBA color;
    color.r=1;color.g=0;color.b=0;color.a=1;
    points.colors.push_back(color);
    


    current_goal_position_visual_pub.publish(points);



   
}
